// --------------------------------------------------------------------------------------------------------------------
// <copyright file="InverseKinematicsFilterWeightedSquaredMaximumMarginFromLimit.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------

namespace Microsoft.Robotics.Kinematics
{
    using System;

    using ShoNS.Array;

    /// <summary>
    /// An IK filter that sorts joint configurations based
    /// on their weighted sum of margins from joint limits.
    /// The score is calculated as the negated sum of the squared distance
    /// to nearest limits weighted by an input factor if given.
    /// In this sense, the smaller the score is, the further the joints
    /// are from their limits, and thus the more flexible in general.
    /// Actual ranking takes place in the base class while
    /// this class provides the score calculator which guides
    /// the ranking process.
    /// </summary>
    public class InverseKinematicsFilterWeightedSquaredMaximumMarginFromLimit : InverseKinematicsFilter
    {
        /// <summary>
        /// A list of joints from which we can query the joint limits
        /// </summary>
        private Joint[] jointList;

        /// <summary>
        /// Whether the distance of each joint to its limit is normalized by the range of the joint
        /// </summary>
        private bool normalizeDistanceByJointRange;

        /// <summary>
        /// Weight factors to be applied to the distance to the nearest limit of each joint
        /// </summary>
        private double[] distanceWeights;

        /// <summary>
        /// Initializes a new instance of the <see cref="InverseKinematicsFilterWeightedSquaredMaximumMarginFromLimit" /> class.
        /// </summary>
        /// <param name="jointList">The list of joints this IK filter will work on</param>
        /// <param name="normalizeByRange">Whether we would like to normalize the distance to the nearest limit by the entire range of the corresponding joint</param>
        /// <param name="weights">The weights to apply to the distance of each joint. This is ignored if it is set to null</param>
        public InverseKinematicsFilterWeightedSquaredMaximumMarginFromLimit(Joint[] jointList, bool normalizeByRange = true, double[] weights = null)
        {
            if (weights != null && weights.Length != jointList.Length)
            {
                throw new ArgumentException("Expecting same length of jointList and weights");
            }

            this.jointList = jointList;
            this.normalizeDistanceByJointRange = normalizeByRange;
            this.distanceWeights = weights;
        }

        /// <summary>
        /// Compute the score of the joint configuration by examining the joint value and the joint limits.
        /// The score is the weighted sum of the negated distance of each joint to their corresponding nearest joint limit.
        /// The distance would be normalized by the joint range if specified.
        /// </summary>
        /// <param name="jointConfiguration">A joint configuration</param>
        /// <returns>A score to evaluate the weighted margin of the joint configuration from joint limits</returns>
        public override double ScoreCalculator(DoubleArray jointConfiguration)
        {
            double distance = 0;
            for (int i = 0; i < jointConfiguration.Length; ++i)
            {
                // calculate the distance to the nearest joint limit
                double distanceToMax = Math.Abs(jointConfiguration[i] - this.jointList[i].MaxValue);
                double distanceToMin = Math.Abs(jointConfiguration[i] - this.jointList[i].MinValue);
                double range = this.jointList[i].MaxValue - this.jointList[i].MinValue;

                double offset = Math.Min(distanceToMin, distanceToMax);

                // if we want to normalize the distance by the joint range
                if (this.normalizeDistanceByJointRange)
                {
                    offset = offset / range;
                }

                if (null == this.distanceWeights)
                {
                    distance -= offset * offset;
                }
                else
                {
                    distance -= offset * offset * this.distanceWeights[i];
                }
            }

            return distance;
        }
    }
}
